Add hooks to save/restore AMU context for Cortex A75

Change-Id: I504d3f65ca5829bc1f4ebadb764931f8379ee81f
Signed-off-by: Dimitris Papastamos <[email protected]>

diff --git a/include/lib/cpus/aarch64/cortex_a75.h b/include/lib/cpus/aarch64/cortex_a75.h
index d68c95721369ba0de6d02e6ba2520b8a1364c467..940125dafd1aa1e10e065ea7e35cdffcb646ca37 100644 (file)
--- a/include/lib/cpus/aarch64/cortex_a75.h
+++ b/include/lib/cpus/aarch64/cortex_a75.h
@@ -50,7 +50,19 @@
  * CPUAMEVTYPER<n> register and are disabled by default. Platforms may
  * enable this with suitable programming.
  */
+#define CORTEX_A75_AMU_NR_COUNTERS     5
 #define CORTEX_A75_AMU_GROUP0_MASK     0x7
 #define CORTEX_A75_AMU_GROUP1_MASK     (0 << 3)
 
+#ifndef __ASSEMBLY__
+#include <stdint.h>
+
+uint64_t cortex_a75_amu_cnt_read(int idx);
+void cortex_a75_amu_cnt_write(int idx, uint64_t val);
+unsigned int cortex_a75_amu_read_cpuamcntenset_el0(void);
+unsigned int cortex_a75_amu_read_cpuamcntenclr_el0(void);
+void cortex_a75_amu_write_cpuamcntenset_el0(unsigned int mask);
+void cortex_a75_amu_write_cpuamcntenclr_el0(unsigned int mask);
+#endif /* __ASSEMBLY__ */
+
 #endif /* __CORTEX_A75_H__ */

diff --git a/lib/cpus/aarch64/cortex_a75.S b/lib/cpus/aarch64/cortex_a75.S
index 4cab9e4fccd88dbef22331f5eae588358937658f..7bd20202313333c3de5e32fc5bbb5a6a8fd6667a 100644 (file)
--- a/lib/cpus/aarch64/cortex_a75.S
+++ b/lib/cpus/aarch64/cortex_a75.S
@@ -11,6 +11,104 @@
 #include <plat_macros.S>
 #include <cortex_a75.h>
 
+       .globl  cortex_a75_amu_cnt_read
+       .globl  cortex_a75_amu_cnt_write
+       .globl  cortex_a75_amu_read_cpuamcntenset_el0
+       .globl  cortex_a75_amu_read_cpuamcntenclr_el0
+       .globl  cortex_a75_amu_write_cpuamcntenset_el0
+       .globl  cortex_a75_amu_write_cpuamcntenclr_el0
+
+/*
+ * uint64_t cortex_a75_amu_cnt_read(int idx);
+ *
+ * Given `idx`, read the corresponding AMU counter
+ * and return it in `x0`.
+ */
+func cortex_a75_amu_cnt_read
+       adr     x1, 1f
+       lsl     x0, x0, #3
+       add     x1, x1, x0
+       br      x1
+
+1:
+       mrs     x0, CPUAMEVCNTR0_EL0
+       ret
+       mrs     x0, CPUAMEVCNTR1_EL0
+       ret
+       mrs     x0, CPUAMEVCNTR2_EL0
+       ret
+       mrs     x0, CPUAMEVCNTR3_EL0
+       ret
+       mrs     x0, CPUAMEVCNTR4_EL0
+       ret
+endfunc cortex_a75_amu_cnt_read
+
+/*
+ * void cortex_a75_amu_cnt_write(int idx, uint64_t val);
+ *
+ * Given `idx`, write `val` to the corresponding AMU counter.
+ */
+func cortex_a75_amu_cnt_write
+       adr     x2, 1f
+       lsl     x0, x0, #3
+       add     x2, x2, x0
+       br      x2
+
+1:
+       msr     CPUAMEVCNTR0_EL0, x0
+       ret
+       msr     CPUAMEVCNTR1_EL0, x0
+       ret
+       msr     CPUAMEVCNTR2_EL0, x0
+       ret
+       msr     CPUAMEVCNTR3_EL0, x0
+       ret
+       msr     CPUAMEVCNTR4_EL0, x0
+       ret
+endfunc cortex_a75_amu_cnt_write
+
+/*
+ * unsigned int cortex_a75_amu_read_cpuamcntenset_el0(void);
+ *
+ * Read the `CPUAMCNTENSET_EL0` CPU register and return
+ * it in `x0`.
+ */
+func cortex_a75_amu_read_cpuamcntenset_el0
+       mrs     x0, CPUAMCNTENSET_EL0
+       ret
+endfunc cortex_a75_amu_read_cpuamcntenset_el0
+
+/*
+ * unsigned int cortex_a75_amu_read_cpuamcntenclr_el0(void);
+ *
+ * Read the `CPUAMCNTENCLR_EL0` CPU register and return
+ * it in `x0`.
+ */
+func cortex_a75_amu_read_cpuamcntenclr_el0
+       mrs     x0, CPUAMCNTENCLR_EL0
+       ret
+endfunc cortex_a75_amu_read_cpuamcntenclr_el0
+
+/*
+ * void cortex_a75_amu_write_cpuamcntenset_el0(unsigned int mask);
+ *
+ * Write `mask` to the `CPUAMCNTENSET_EL0` CPU register.
+ */
+func cortex_a75_amu_write_cpuamcntenset_el0
+       msr     CPUAMCNTENSET_EL0, x0
+       ret
+endfunc cortex_a75_amu_write_cpuamcntenset_el0
+
+/*
+ * void cortex_a75_amu_write_cpuamcntenclr_el0(unsigned int mask);
+ *
+ * Write `mask` to the `CPUAMCNTENCLR_EL0` CPU register.
+ */
+func cortex_a75_amu_write_cpuamcntenclr_el0
+       mrs     x0, CPUAMCNTENCLR_EL0
+       ret
+endfunc cortex_a75_amu_write_cpuamcntenclr_el0
+
 func cortex_a75_reset_func
 #if ENABLE_AMU
        /* Make sure accesses from EL0/EL1 and EL2 are not trapped to EL3 */

diff --git a/lib/cpus/aarch64/cortex_a75_pubsub.c b/lib/cpus/aarch64/cortex_a75_pubsub.c
new file mode 100644 (file)
index 0000000..c1089a6
--- /dev/null
+++ b/lib/cpus/aarch64/cortex_a75_pubsub.c
@@ -0,0 +1,75 @@
+/*
+ * Copyright (c) 2017, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <cortex_a75.h>
+#include <pubsub_events.h>
+#include <platform.h>
+
+struct amu_ctx {
+       uint64_t cnts[CORTEX_A75_AMU_NR_COUNTERS];
+       uint16_t mask;
+};
+
+static struct amu_ctx amu_ctxs[PLATFORM_CORE_COUNT];
+
+static void *cortex_a75_context_save(const void *arg)
+{
+       struct amu_ctx *ctx = &amu_ctxs[plat_my_core_pos()];
+       unsigned int midr;
+       unsigned int midr_mask;
+       int i;
+
+       midr = read_midr();
+       midr_mask = (MIDR_IMPL_MASK << MIDR_IMPL_SHIFT) |
+               (MIDR_PN_MASK << MIDR_PN_SHIFT);
+       if ((midr & midr_mask) != (CORTEX_A75_MIDR & midr_mask))
+               return 0;
+
+       /* Save counter configuration */
+       ctx->mask = cortex_a75_amu_read_cpuamcntenset_el0();
+
+       /* Ensure counters are disabled */
+       cortex_a75_amu_write_cpuamcntenclr_el0(ctx->mask);
+       isb();
+
+       /* Save counters */
+       for (i = 0; i < CORTEX_A75_AMU_NR_COUNTERS; i++)
+               ctx->cnts[i] = cortex_a75_amu_cnt_read(i);
+
+       return 0;
+}
+
+static void *cortex_a75_context_restore(const void *arg)
+{
+       struct amu_ctx *ctx = &amu_ctxs[plat_my_core_pos()];
+       unsigned int midr;
+       unsigned int midr_mask;
+       int i;
+
+       midr = read_midr();
+       midr_mask = (MIDR_IMPL_MASK << MIDR_IMPL_SHIFT) |
+               (MIDR_PN_MASK << MIDR_PN_SHIFT);
+       if ((midr & midr_mask) != (CORTEX_A75_MIDR & midr_mask))
+               return 0;
+
+       ctx = &amu_ctxs[plat_my_core_pos()];
+
+       /* Counters were disabled in `cortex_a75_context_save()` */
+       assert(cortex_a75_amu_read_cpuamcntenset_el0() == 0);
+
+       /* Restore counters */
+       for (i = 0; i < CORTEX_A75_AMU_NR_COUNTERS; i++)
+               cortex_a75_amu_cnt_write(i, ctx->cnts[i]);
+       isb();
+
+       /* Restore counter configuration */
+       cortex_a75_amu_write_cpuamcntenset_el0(ctx->mask);
+
+       return 0;
+}
+
+SUBSCRIBE_TO_EVENT(psci_suspend_pwrdown_start, cortex_a75_context_save);
+SUBSCRIBE_TO_EVENT(psci_suspend_pwrdown_finish, cortex_a75_context_restore);

diff --git a/plat/arm/board/fvp/platform.mk b/plat/arm/board/fvp/platform.mk
index 7edbd3dfa1466b571be40160d1f5b922d79903b1..9d3c5f6b7ec32c3ddd366e48ca37a5def9345158 100644 (file)
--- a/plat/arm/board/fvp/platform.mk
+++ b/plat/arm/board/fvp/platform.mk
@@ -150,6 +150,10 @@ ENABLE_PLAT_COMPAT :=      0
 # Enable Activity Monitor Unit extensions by default
 ENABLE_AMU                     :=      1
 
+ifeq (${ENABLE_AMU},1)
+BL31_SOURCES           += lib/cpus/aarch64/cortex_a75_pubsub.c
+endif
+
 ifneq (${ENABLE_STACK_PROTECTOR},0)
 PLAT_BL_COMMON_SOURCES +=      plat/arm/board/fvp/fvp_stack_protector.c
 endif